#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     IR1200,         sensorHiTechnicIRSeeker1200)
#pragma config(Sensor, S3,     Light,          sensorLightActive)
#pragma config(Sensor, S4,     Touch,          sensorTouch)
#pragma config(Motor,  mtr_S1_C1_1,     MiddleMotor,   tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     motorE,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_1,     LeftMotor,     tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     RightMotor,    tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Servo,  srvo_S1_C3_1,    servo1,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_2,    servo2,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

int white = 1000;
int black = -1000;

void turnL (int power, int duration)
{
  motor[LeftMotor]= -1*power;
  motor[RightMotor]= power;
  wait1Msec(duration);
}

void Forward (int power, int duration)
{
  motor[LeftMotor]= power;
  motor[RightMotor]= power;
  wait1Msec(duration);
}

void turnR (int power, int duration)
{
  motor[LeftMotor]= power;
  motor[RightMotor]= -1*power;
  wait1Msec(duration);
}

void Backward (int power, int duration)
{
  motor[LeftMotor]= -1*power;
  motor[RightMotor]= -1*power;
  wait1Msec(duration);
}

void ProportionalLineFollowing ()
{
	while(SensorValue[Light] >=  && SensorValue[Light] <= )
	{
		motor[LeftMotor] = 0;
		motor[RightMotor] = 30;
		wait1Msec(50);
	}
	while(SensorValue[Light] >  && SensorValue[Light] <= )
	{
		motor[LeftMotor] = 8;
		motor[RightMotor] = 22;
		wait1Msec(50);
	}
	while(SensorValue[Light] >  && SensorValue[Light] <= )
	{
		motor[LeftMotor] = 15;
		motor[RightMotor] = 15;
		wait1Msec(50);
	}
	while(SensorValue[Light] >  && SensorValue[Light] <= )
	{
		motor[LeftMotor] = 22;
		motor[RightMotor] = 8;
		wait1Msec(50);
	}
	while(SensorValue[Light] >  && SensorValue[Light] <= )
	{
		motor[LeftMotor] = 30;
		motor[RightMotor] = 0;
		wait1Msec(50);
	}
}

void LightSensorCalibration()
{
	nxtDisplayCenteredTextLine(3, "BLACK");
	wait1Msec(1000);
	nxtDisplayCenteredTextLine(3,"RIGHT");
	wait1Msec(1000);
	nxtDisplayCenteredTextLine(3,"FIRST");
	wait1Msec(1000);
	nxtDisplayCenteredTextLine(3, "WHITE");
	wait1Msec(1000);
	nxtDisplayCenteredTextLine(3,"LEFT");
	wait1Msec(1000);
	nxtDisplayCenteredTextLine(3,"SECOND");
	wait1Msec(1000);
	while(nNxtButtonPressed != 3)
  {
	  if(nNxtButtonPressed == 1)
	  {
	  	black = SensorValue[Light];
			nxtDisplayCenteredTextLine(3,"BLACK %d", black);
	  }
	  else if(nNxtButtonPressed == 2)
	  {
	  	white = SensorValue[Light];
	  	nxtDisplayCenteredTextLine(3, "WHITE %d", white);
	  }
	}
}

task main()
{
	LightSensorCalibration();
	while (true)
	{
		ProportionalLineFollowing();
	}
}
